#include "ros/ros.h"
#include "robot_control/Motion.h"
#include "robot_control/Odometry.h"
#include "robot_control/Trajectory.h"
#include "robot_control/CameraPose.h"
#include <fcntl.h> 
#include <termios.h>
#include <math.h>


#include <gsl/gsl_rng.h>		// gsl library for random number generator
#include <gsl/gsl_cdf.h>		// gsl library for
#include <gsl/gsl_matrix.h>		// gsl library for matrix
#include <gsl/gsl_vector.h>		// gsl library for vector
#include <gsl/gsl_blas.h>		// gsl library for matrix operations
#include <gsl/gsl_linalg.h>	// gsl library for linear algebra
#include <gsl/gsl_randist.h>

#define REPEAT_RATE 50
#define ACCELERATION 30//50
#define ACCELERATION_ROTATION 25//25

int count_recv_msg_time = 0;

int comport;
double encoder_resolusion = 150;
double gearbox = 75;
double wheel_radius = 0.07;//m

int base_encoder_val_0;
int base_encoder_val_1;
bool encoder_init= false;
long int initial_enc0 = 0;
long int initial_enc1 = 0;
bool initial ;
//TODO(jingjing) take this from parameter server
double wheelbase = 0.4; 
double pos_old_x = 0.0;
double pos_old_y = 0.0;
long int pre_speed = 0;
long int pre_rotation = 0;

double camera_x = 0;
double camera_y = 0;
double camera_rot = 0;
bool camera_reset = true;

int one_round_enc0, one_round_enc1;
bool toggle=false;
FILE *odometry_file;

ros::Time last_probe_time;

int accept_priority = 3;

double delta_enc0 = 0.0;
double delta_enc1 = 0.0;

typedef struct
{
	double x;
	double y;
} vector2_t;

typedef struct
{
	vector2_t pos;
	double rot;
} pose2_t;

double previous_enc0;
double previous_enc1;
int i=0;
bool speed_reset = false;
bool rotation_reset = false;

int odometry_id_ = 0;

void CameraPoseCallback(const robot_control::CameraPose::ConstPtr& msg) {
	camera_x = msg->x;
	camera_y = msg->y;
	camera_rot = msg->rot;
	camera_reset = true;
	ROS_INFO("Received new camera pose %lf %lf %lf", camera_x, camera_y, camera_rot * 180 /3.141);

}

void SpeedChange(int64_t speed)
{
	int ret_val;
	std::stringstream speed_command1;

	speed_command1 << "v " << speed << "\r\n";

	//ROS_INFO("Set speed to: %ld", (long) speed);

	ret_val = write(comport, speed_command1.str().c_str(), 
			speed_command1.str().size());  
	if (ret_val <= 0)
		ROS_ERROR("Cannot write speed command on robot");
}

void RotationChange(int64_t rotation)
{
	int ret_val;
	std::stringstream rot_command;

	rot_command << "r " << rotation << "\r\n";
	//ROS_INFO("Set rotation to: %ld", (long) rotation);

	ret_val = write(comport, rot_command.str().c_str(), rot_command.str().size());
	if (ret_val <= 0)
		ROS_ERROR("Cannot write rotation command on robot");
}



void M1Change(int64_t speed)
{
	std::stringstream speed_command;
	speed_command << "m 0 " << speed << "\r\n"; 

	int ret_val;

	ret_val = write(comport, speed_command.str().c_str(), speed_command.str().size());  
	ROS_DEBUG("%s", speed_command.str().c_str());
	if (ret_val <= 0)
		ROS_ERROR("Cannot write speed command on robot");	
}

void M2Change(int64_t speed)
{
	std::stringstream speed_command;
	speed_command << "m 1 " << speed << "\r\n"; 

	int ret_val;

	ret_val = write(comport, speed_command.str().c_str(), speed_command.str().size());  
	ROS_DEBUG("%s", speed_command.str().c_str());
	if (ret_val <= 0)
		ROS_ERROR("Cannot write speed command on robot");	
}

void MotionControllerCallback(const robot_control::Motion::ConstPtr& msg)
{
	//ROS_INFO("Motion command received: Speed: %ld Rotation: %ld",
	//			(long int) msg->speed, (long int) msg->rotation); 

	if (msg->priority > accept_priority) {
		return;
	}

	if (msg->reset_priority) {
		accept_priority = msg->reset_priority;
		ROS_ERROR("Priority lowered to %d", accept_priority);
		return;
	}

	if (msg->priority < accept_priority) {
		accept_priority = msg->priority;
		ROS_ERROR("Priority raised to %d", accept_priority);
	}

	if(delta_enc0 == 0 && delta_enc1 == 0 && initial == false) {
		count_recv_msg_time++;
	}

	if(count_recv_msg_time % REPEAT_RATE == 0 && count_recv_msg_time != 0
			&& ((msg->speed == pre_speed && msg->rotation == 0) ||
				(msg->rotation == pre_rotation && msg->speed == 0))  &&
			msg->computing_path == 1000)
	{

		if(msg->rotation == pre_rotation && msg->speed == 0)
		{ pre_rotation = ACCELERATION;
			ROS_INFO("rotation block, reset");
		}
		else if(msg->speed == pre_speed && msg->rotation == 0)
		{pre_speed = ACCELERATION;
			ROS_INFO("speed block, reset");
		}
		SpeedChange(ACCELERATION);
		count_recv_msg_time = 0;
	}

	if(msg->speed != pre_speed ){
		SpeedChange(msg->speed);
		pre_speed = msg->speed;
	}

	if(msg->rotation != pre_rotation){
		RotationChange(msg->rotation);	
		pre_rotation = msg->rotation;
	}


}

void serial_port_init(void){/*initalize serial port 38400BPS*/
	struct termios options;
	ROS_INFO("Init serial port");
	comport = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);

	if(comport == -1) // if open is unsucessful
	{
		ROS_FATAL("open_port: Unable to open /dev/ttyS0. \n");
	}
	else
	{
		fcntl(comport, F_SETFL, 0);
		ROS_INFO("port is open");
	}

	/*     * Get the current options for the port...     */
	tcgetattr(comport, &options);
	/*     * Set the baud rates to 19200...     */
	cfsetispeed(&options, B38400);
	cfsetospeed(&options, B38400);
	/*     * Enable the receiver and set local mode...     */
	options.c_cflag |= (CLOCAL | CREAD);
	/*     * Set the new options for the port...     */
	tcsetattr(comport, TCSANOW, &options);
	std::stringstream initial_command;
	initial_command << "\n"; 

	int ret_val;

	ret_val = write(comport, initial_command.str().c_str(), initial_command.str().size());  


}

void serial_port_close(void)
{
	close(comport);
}
#define READ_BUF_SIZE 200
bool read_odometry(robot_control::Odometry *msg)
{
	int ret_val;

	std::stringstream query_odometry_command;
	query_odometry_command << "enc\r\n";//enc
	ret_val = write(comport, query_odometry_command.str().c_str(),
			query_odometry_command.str().size());  
	if (ret_val <= 0)
		ROS_ERROR("Cannot write speed command on robot");

	char buffer[READ_BUF_SIZE];

	int read_bytes = read(comport, buffer, READ_BUF_SIZE);
	if (read_bytes <= 0) {
		ROS_ERROR("Cannot read odometry");
		return false;
	}
	buffer[read_bytes] = '\0';
	ROS_DEBUG("Buffer: %s", buffer);

	ret_val = sscanf(buffer,
			"enc\nEncoder 0 pos: %ld\n"
			"Encoder 1 pos: %ld",(long int *) &(msg->enc0), (long int *) &(msg->enc1));

	if (ret_val != 2) {
		ROS_ERROR("Cannot parse odometry");
		return false;
	}

	if(initial){
		initial_enc0 = msg->enc0;
		initial_enc1 = msg->enc1;
		ROS_INFO("Initial read enc0 %ld, enc1 %ld", 
				(long int) initial_enc0, (long int) initial_enc1);
	}

	return true;

}


void compute_odometry(const robot_control::Odometry& msg,
		robot_control::Trajectory *pose)//check enc0 is left?
{
	ros::Duration time_elapsed = ros::Time::now() - last_probe_time;
	last_probe_time = ros::Time::now();

	double angular_position[2];
	double line_position[2];
	double delta_distance;
	pose2_t delta;

	if(initial){
		previous_enc0 = msg.enc0;
		previous_enc1 = msg.enc1;
		initial = false;
		ROS_INFO("Initial or reset the encoder");
	}

	if(camera_reset) {
		ROS_INFO("Not corrected pose was %lf %lf %lf", pose->x, pose->y,
				pose->rot * 180/3.141);
		pose->x = camera_x;
		pose->y = camera_y;
		pose->rot = camera_rot;
		camera_reset = false;
	}
	delta_enc0 = msg.enc0 - previous_enc0;
	delta_enc1 = msg.enc1 - previous_enc1;

	if(delta_enc0 != 0 || delta_enc1 != 0 ){
		previous_enc0 = msg.enc0;
		previous_enc1 = msg.enc1;

		angular_position[0] = (double) delta_enc0 / (encoder_resolusion * gearbox);
		angular_position[1] = (double) delta_enc1 / (encoder_resolusion * gearbox);
		line_position[0] =  wheel_radius * angular_position[0] * 2 * M_PI ;
		line_position[1] =  wheel_radius * angular_position[1] * 2 * M_PI; 

		delta_distance = (line_position[0] + line_position[1])/2;

		delta.rot =(line_position[0] - line_position[1])/wheelbase;//1-0 or 0-1

		pose->velocity = delta_distance / time_elapsed.toSec();
		pose->rotation_speed = delta.rot / time_elapsed.toSec();
		pose->time_elapsed = time_elapsed.toSec();

		pose->rot += delta.rot;
		delta.pos.x = delta_distance * cos(pose->rot);
		delta.pos.y = delta_distance * sin(pose->rot);


		pose->x += delta.pos.x;
		pose->y += delta.pos.y;
		fprintf(odometry_file, "VERTEX2 %d %f %f %f\n", odometry_id_, pose->x, pose->y, pose->rot);
		odometry_id_++;
	}


}


void encoder_testing(robot_control::Odometry *msg)
{

	one_round_enc0 = 75*150;
	one_round_enc1 = 75*150;

	if(!encoder_init){
		base_encoder_val_0 = msg->enc0;
		base_encoder_val_1 = msg->enc1;
		encoder_init = true;

	}

	if(encoder_init){

		int rel_enc_val_0 = msg->enc0-base_encoder_val_0;
		int rel_enc_val_1 = msg->enc1-base_encoder_val_1;
		ROS_INFO("!encoder_init : base_encoder %d, enc1 %d, rel_enc_val %d, %d, toggle %d" ,
				base_encoder_val_0, base_encoder_val_1, rel_enc_val_0,rel_enc_val_1,toggle);


		if(toggle){
			if((rel_enc_val_0) > (one_round_enc0+1000))
				M2Change(-30);
			else if((rel_enc_val_0) < (one_round_enc0-1000))
				M2Change(30);
			else
				M2Change(0);


			toggle = false;
		}else{
			if((rel_enc_val_1) > (one_round_enc1+1000))
				M1Change(-30);
			else if((rel_enc_val_1) < (one_round_enc1-1000))
				M1Change(30);
			else
				M1Change(0);

			toggle = true;
		}

	}
}


int main(int argc, char **argv)
{
	ros::init(argc, argv, "motion_controller");
	serial_port_init();

	odometry_file = fopen("odometry_trajectory.launch","w+");

	ros::NodeHandle n;
	ros::Subscriber sub_motion = n.subscribe("motion_control", 1000,
			MotionControllerCallback);
	ros::Subscriber sub_camera = n.subscribe("camera_pose", 1000,
			CameraPoseCallback);
	ros::Publisher odometry_pub = n.advertise<robot_control::Odometry>(
			"robot_odometry", 1000);

	ros::Publisher trajectory_pub = n.advertise<robot_control::Trajectory>(
			"robot_trajectory", 1000);

	RotationChange(0);
	SpeedChange(0);
	initial = true;
	ros::Rate loop_rate(10);
	robot_control::Odometry msg; 
	robot_control::Trajectory traj_msg; 
	last_probe_time = ros::Time::now();

	while (ros::ok())
	{

		bool odo_ok = read_odometry(&msg);  

		if (odo_ok){
			compute_odometry(msg, &traj_msg);
			traj_msg.priority = accept_priority;
			odometry_pub.publish(msg);
			trajectory_pub.publish(traj_msg);
		}

		ros::spinOnce();
		loop_rate.sleep();
	}
	ROS_INFO("Stopping robot");
	SpeedChange(0);
	RotationChange(0);
	serial_port_close();
	return 0;
}

